/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file    stm32h7xx_it.c
  * @brief   Interrupt Service Routines.
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32h7xx_it.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "74HC4051PW.h"
#include "applInterface.h"
#include "arm_math.h"
#include "Motor_Control.h"
#include "usart.h"
#include "encoder.h"
#include "pid.h"
#include "SEGGER_RTT.h"
#include "tim.h"
#include "Force_Sensor.h"
#include "Speed_Control.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN TD */

/* USER CODE END TD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
float a = 0;
float angle = 0;
uint8_t flag = 0;
uint8_t Position_Flag = 0;
uint8_t Speed_Flag = 0;
uint32_t Tim_7_Counter_1hz = 0;
uint32_t Tim_7_Counter_200hz = 0;
uint32_t Tim_7_Counter_1000hz = 0;
uint32_t Tim_7_Counter = 0;
float max_velocity = 0;
char temp[128];
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/* External variables --------------------------------------------------------*/
extern ADC_HandleTypeDef hadc1;
extern ADC_HandleTypeDef hadc3;
extern TIM_HandleTypeDef htim7;
extern TIM_HandleTypeDef htim17;
extern UART_HandleTypeDef huart5;
/* USER CODE BEGIN EV */

/* USER CODE END EV */

/******************************************************************************/
/*           Cortex Processor Interruption and Exception Handlers          */
/******************************************************************************/
/**
  * @brief This function handles Non maskable interrupt.
  */
void NMI_Handler(void)
{
  /* USER CODE BEGIN NonMaskableInt_IRQn 0 */

  /* USER CODE END NonMaskableInt_IRQn 0 */
  /* USER CODE BEGIN NonMaskableInt_IRQn 1 */
   while (1)
  {
  }
  /* USER CODE END NonMaskableInt_IRQn 1 */
}

/**
  * @brief This function handles Hard fault interrupt.
  */
void HardFault_Handler(void)
{
  /* USER CODE BEGIN HardFault_IRQn 0 */

  /* USER CODE END HardFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_HardFault_IRQn 0 */
    /* USER CODE END W1_HardFault_IRQn 0 */
  }
}

/**
  * @brief This function handles Memory management fault.
  */
void MemManage_Handler(void)
{
  /* USER CODE BEGIN MemoryManagement_IRQn 0 */

  /* USER CODE END MemoryManagement_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
    /* USER CODE END W1_MemoryManagement_IRQn 0 */
  }
}

/**
  * @brief This function handles Pre-fetch fault, memory access fault.
  */
void BusFault_Handler(void)
{
  /* USER CODE BEGIN BusFault_IRQn 0 */

  /* USER CODE END BusFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_BusFault_IRQn 0 */
    /* USER CODE END W1_BusFault_IRQn 0 */
  }
}

/**
  * @brief This function handles Undefined instruction or illegal state.
  */
void UsageFault_Handler(void)
{
  /* USER CODE BEGIN UsageFault_IRQn 0 */

  /* USER CODE END UsageFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_UsageFault_IRQn 0 */
    /* USER CODE END W1_UsageFault_IRQn 0 */
  }
}

/**
  * @brief This function handles System service call via SWI instruction.
  */
void SVC_Handler(void)
{
  /* USER CODE BEGIN SVCall_IRQn 0 */

  /* USER CODE END SVCall_IRQn 0 */
  /* USER CODE BEGIN SVCall_IRQn 1 */

  /* USER CODE END SVCall_IRQn 1 */
}

/**
  * @brief This function handles Debug monitor.
  */
void DebugMon_Handler(void)
{
  /* USER CODE BEGIN DebugMonitor_IRQn 0 */

  /* USER CODE END DebugMonitor_IRQn 0 */
  /* USER CODE BEGIN DebugMonitor_IRQn 1 */

  /* USER CODE END DebugMonitor_IRQn 1 */
}

/**
  * @brief This function handles Pendable request for system service.
  */
void PendSV_Handler(void)
{
  /* USER CODE BEGIN PendSV_IRQn 0 */

  /* USER CODE END PendSV_IRQn 0 */
  /* USER CODE BEGIN PendSV_IRQn 1 */

  /* USER CODE END PendSV_IRQn 1 */
}

/**
  * @brief This function handles System tick timer.
  */
void SysTick_Handler(void)
{
  /* USER CODE BEGIN SysTick_IRQn 0 */

  /* USER CODE END SysTick_IRQn 0 */
  HAL_IncTick();
  /* USER CODE BEGIN SysTick_IRQn 1 */

  /* USER CODE END SysTick_IRQn 1 */
}

/******************************************************************************/
/* STM32H7xx Peripheral Interrupt Handlers                                    */
/* Add here the Interrupt Handlers for the used peripherals.                  */
/* For the available peripheral interrupt handler names,                      */
/* please refer to the startup file (startup_stm32h7xx.s).                    */
/******************************************************************************/

/**
  * @brief This function handles EXTI line1 interrupt.
  */
void EXTI1_IRQHandler(void)
{
  /* USER CODE BEGIN EXTI1_IRQn 0 */

  /* USER CODE END EXTI1_IRQn 0 */
  HAL_GPIO_EXTI_IRQHandler(ECAT_IRQ_Pin);
  /* USER CODE BEGIN EXTI1_IRQn 1 */

  /* USER CODE END EXTI1_IRQn 1 */
}

/**
  * @brief This function handles ADC1 and ADC2 global interrupts.
  */
void ADC_IRQHandler(void)
{
  /* USER CODE BEGIN ADC_IRQn 0 */

  /* USER CODE END ADC_IRQn 0 */
  HAL_ADC_IRQHandler(&hadc1);
  /* USER CODE BEGIN ADC_IRQn 1 */

  /* USER CODE END ADC_IRQn 1 */
}

/**
  * @brief This function handles EXTI line[15:10] interrupts.
  */
void EXTI15_10_IRQHandler(void)
{
  /* USER CODE BEGIN EXTI15_10_IRQn 0 */

  /* USER CODE END EXTI15_10_IRQn 0 */
  HAL_GPIO_EXTI_IRQHandler(HALL_5VSW1_Pin);
  HAL_GPIO_EXTI_IRQHandler(ECAT_SYNC0_Pin);
  HAL_GPIO_EXTI_IRQHandler(ECAT_SYNC1_Pin);
  /* USER CODE BEGIN EXTI15_10_IRQn 1 */

  /* USER CODE END EXTI15_10_IRQn 1 */
}

/**
  * @brief This function handles UART5 global interrupt.
  */
void UART5_IRQHandler(void)
{
  /* USER CODE BEGIN UART5_IRQn 0 */

  /* USER CODE END UART5_IRQn 0 */
  HAL_UART_IRQHandler(&huart5);
  /* USER CODE BEGIN UART5_IRQn 1 */

  /* USER CODE END UART5_IRQn 1 */
}

/**
  * @brief This function handles TIM7 global interrupt.
  */
void TIM7_IRQHandler(void)
{
  /* USER CODE BEGIN TIM7_IRQn 0 */

  /* USER CODE END TIM7_IRQn 0 */
  HAL_TIM_IRQHandler(&htim7);
  /* USER CODE BEGIN TIM7_IRQn 1 */

  /* USER CODE END TIM7_IRQn 1 */
}

/**
  * @brief This function handles TIM17 global interrupt.
  */
void TIM17_IRQHandler(void)
{
  /* USER CODE BEGIN TIM17_IRQn 0 */

  /* USER CODE END TIM17_IRQn 0 */
  HAL_TIM_IRQHandler(&htim17);
  /* USER CODE BEGIN TIM17_IRQn 1 */

  /* USER CODE END TIM17_IRQn 1 */
}

/**
  * @brief This function handles ADC3 global interrupt.
  */
void ADC3_IRQHandler(void)
{
  /* USER CODE BEGIN ADC3_IRQn 0 */

  /* USER CODE END ADC3_IRQn 0 */
  HAL_ADC_IRQHandler(&hadc3);
  /* USER CODE BEGIN ADC3_IRQn 1 */

  /* USER CODE END ADC3_IRQn 1 */
}

/* USER CODE BEGIN 1 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
  if (huart -> Instance == UART5)
  {
    if(rx_data == '!')
    {
      rx_flag = 1;
    }
    else if(rx_data == '?')
    {
      rx_flag = 0;
      data_flag = 0;
      int temp_data = 0;
      sscanf((char *)rx_buffer,"%f",&Target_Position);
      //VQ.target = (float)temp_data;
      memset(rx_buffer,0,sizeof(rx_buffer));
    }
    else if(rx_flag == 1)
    {
      rx_buffer[data_flag] = rx_data;
      data_flag++;
    }
    HAL_UART_Receive_IT(&huart5,&rx_data,1);
  }
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  if(htim -> Instance == TIM7)//10Khz
  {
    Encoder_Data.Pre = Encoder_Data.Now;
    Encoder_Data.Now = Encoder_Read(ANGLEUNC);

    if (Encoder_Data.Pre - Encoder_Data.Now > 0.8 * 16384)
    {
      Circle_Num++;
    }
    else if (Encoder_Data.Now - Encoder_Data.Pre > 0.8 * 16384)
    {
      Circle_Num--;
    }

    // Encoder_Data_Real.Pre = Encoder_Data_Real.Now;
    Encoder_Data_Real.Now = Circle_Num * 16384.0f + Encoder_Data.Now;

    for (uint8_t i = 19; i > 0; i--)
    {
      Encoder_Data_Real_Buffer[i] = Encoder_Data_Real_Buffer[i-1];
    }

    Encoder_Data_Real_Buffer[0] = Encoder_Data_Real.Now;

    if (flag <= 20)
    {
      flag ++;
    }
    else
    {
      float Speed_Data_Temp = (Encoder_Data_Real_Buffer[0] - Encoder_Data_Real_Buffer[19]) / 16384 * 500;
      Speed_Data = first_order_filter_SpeedData(Speed_Data_Temp,0.1f);
    }

//     Tim_7_Counter_1000hz++;
//     if (Tim_7_Counter_1000hz > 1000)//1Khz
//     {
//       Tim_7_Counter_1000hz = 0;
//       angle = angle + 2 * PI / 1000;
//       a = arm_sin_f32(angle) * 60.0f;
//       Speed.target = a ;
//     }
    if(Target_Position != Target_Position_Pre)
    {
      Profile_Generator_Init(&Motor,Target_Position-Encoder_Data_Real.Now,200.0f*16384,50*16384);
    }
    Target_Position_Pre = Target_Position;
    Profile_Generator_Caculate(&Motor);
    Tim_7_Counter_200hz++;
    if (Tim_7_Counter_200hz >= 20)
    {
      Get_Temperature();
      //Temperature_Control();
      Tim_7_Counter_200hz = 0;
//      sprintf(temp,"%.1f,%.1f\n",Current_Velocity,Current_Position);
      sprintf(temp,"%.1f,%.1f\n",Motor.Current_Velocity/16384,Encoder_Data_Real.Now);
//      HAL_UART_Transmit(&huart5,(uint8_t*)temp,strlen(temp),0xffff);
      SEGGER_RTT_printf(0,"%s",temp);
      // SEGGER_RTT_printf(0,"%s\n",Inject_ADC_Value[3]);
      // Voltage_Check();
    }


  }
//  if (htim -> Instance == TIM17)
//  {
//    ECAT_CheckTimer();
//  }
}

void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc)
{
  if (Start_Flag == False)
  {
    //Get the offset of the ADC_Value
    OFFSET_INIT();
  }
  else
  {
    Electrical_Angle = Get_Electrical_Angle();

    Angle_To_Cos_Sin(Electrical_Angle,&TRANSFER_COS_SIN);
    //Detect data from a force sensor
    //Get_Force();
    //
    //Sampling the Current of the 3 phase
    Get_Current_ABC();
    //
    Clarke_Transfer(I_DATA,&I_ALPHA_BETA_DATA);

    Park_Transfer(I_ALPHA_BETA_DATA,TRANSFER_COS_SIN,&Current_DQ);

    Position.target = Motor.Current_Position;
    Position_Flag++;
    if (Position_Flag >= 4)
    {
      Position_Flag = 0;
      Position.f_cal_pid(&Position,Encoder_Data_Real.Now);
      Speed.target = Motor.Current_Velocity/16384+2.0f*Position.output;
    }
    Speed_Flag++;
    if (Speed_Flag >= 2)
    {
      Speed_Flag = 0;
      Speed.f_cal_pid(&Speed,Speed_Data) + 0.00005*Motor.Current_Accelerate;
      VQ.target = first_order_filter_SpeedOutput(Speed.output,0.1f);
    }
    VD.f_cal_pid(&VD,Current_DQ.Id);
    VQ.f_cal_pid(&VQ,Current_DQ.Iq);
    Voltage_DQ.Vd = VD.output;
    Voltage_DQ.Vq = VQ.output;
//     Voltage_DQ.Vq = 0.7f;
//     Voltage_DQ.Vd = 0.0f;

    Rev_Park_Transfer(Voltage_DQ,TRANSFER_COS_SIN,&Voltage_Alpha_Beta);
    SVPWM_Control(Voltage_Alpha_Beta,48,PWM_TIM_PULSE_TPWM);
  }
}

/* USER CODE END 1 */
